#include "rclcpp/rclcpp.hpp"
#include "mavros_msgs/msg/state.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "mavros_msgs/srv/set_mode.hpp"
#include "mavros_msgs/srv/command_bool.hpp"
#include "rclcpp/rclcpp.hpp"
#include "mavros_msgs/msg/state.hpp"
#include "mavros_msgs/srv/command_bool.hpp"
#include "mavros_msgs/srv/set_mode.hpp"
#include "mavros_msgs/msg/position_target.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <std_msgs/msg/bool.hpp>
#include "yolo_detect_message/msg/traker_boxs.hpp"
#include "yolo_detect_message/msg/traker_box.hpp"
#include "yolo_detect_message/msg/detect_boxs.hpp"
#include  "yolo_detect_message/msg/detect_box.hpp"
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <thread>
#include <algorithm>
#include "targer_tracker/PID_Controller.hpp"
#include "targer_tracker/MovingAverageFilter.hpp"
#include <mutex>
#include "yolo_detect_message/srv/service.hpp"
#include "yolo_detect_message/msg/call_server_status.hpp"
#include "client_interfaces/msg/config_target.hpp"
#include "yolo_detect_message/msg/move_interface.hpp"
class target_tracker : public rclcpp::Node
{
private:
    // uav state
    rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr state_sub_;
    // current uav state
    
    std::mutex  current_state_mutex;
    mavros_msgs::msg::State current_state_;
    // uav vellocity state
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_pub_;
    // rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr local_pos_pub_;
    // arming cmd 
    rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arming_client_;
    // arming state
    
    std::mutex arming_mutex;
    bool arming;
    rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr set_mode_client_;

    rclcpp::Subscription<yolo_detect_message::msg::DetectBoxs>::SharedPtr tracker_sub_;
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr target_vel_pub_;
    // rclcpp::Publisher<mavros_msgs::msg::PositionTarget>::SharedPtr raw_setpoint_pub_;

    // service
    rclcpp::CallbackGroup::SharedPtr callback_group_;
    rclcpp::Service<yolo_detect_message::srv::Service>::SharedPtr service_;
    rclcpp::Subscription<yolo_detect_message::msg::CallServerStatus>::SharedPtr service_status_subscriber_;
    rclcpp::Subscription<yolo_detect_message::msg::CallServerStatus>::SharedPtr multi_service_status_subscriber_;
    rclcpp::Publisher<client_interfaces::msg::ConfigTarget>::SharedPtr dingwei_pub_;
    rclcpp::Publisher<yolo_detect_message::msg::MoveInterface>::SharedPtr move_pub_;
    // 无人机追踪状态发布者
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr tracker_status_pub_;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr dingwei_status_;
    std::mutex UAV_position_mutex;
    geometry_msgs::msg::PoseStamped UAV_position;

    std::mutex current_velocity_mutex;
    geometry_msgs::msg::Twist current_velocity;
    bool stop_thread_;  // 控制线程退出标志
    bool controller;

    bool b_arrived;
    int tracker_times;
    int tracker_id;
    // image wight and height
    int w = 1920;
    int h = 1080;
    double target_location_height = 0.0;
    double target_location_width = 0.0;
    bool init_b;
    float delta_t;
    double max_vel = 3.0;
    float height = 10.0;
    PIDController pid_x_, pid_y_;
    MovingAverageFilter filter_x_, filter_y_;

    std::mutex target_pose_mutex;
    geometry_msgs::msg::PoseStamped target_pose_;


    float clamp(float value,float maxvalue);


    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Time last_request_;

    // find object
    bool b_find;

    bool first_find = false;

    int wait_time = 0;

    // uav state sub
    std::string uav_state_sub_topic_;
    int uav_state_sub_queue_size_;

    // tracker sub
    std::string trackerid_sub_topic_;
    int trackerid_sub_queue_size_;

    // velocity pub
    std::string velocity_pub_topic_;
    int velocity_pub_queue_size_;

    // arming client
    std::string arming_client_topic_;
    int arming_client_queue_size_;

    // set mode client
    std::string set_mode_client_topic_;
    int set_mode_client_queue_size_;

    double pid_x__Kp;
    double pid_y__Kp;
    double pid_x__Ki;
    double pid_y__Ki;
    double pid_x__Kd;
    double pid_y__Kd;
    
    std::string detect_server_;
    std::string tracker_name_;
    std::string locaiton_sub_;

    std::mutex service_status_mutex;
    bool service_status =false;

    std::mutex multi_service_status_mutex;
    bool service_status_multi = false;

    std::string call_server_topic_name;
    std::string multi_call_server_topic_name;
    client_interfaces::msg::ConfigTarget dingwei_data;

    int vehcile_id_;
    std::string namespaces;
    std::string dingwei_topic_name;

    double center_x = 0.0;
    double center_y = 0.0;
    // 追踪状态，是否追踪到目标物体 false 未追踪到 true 追踪到
    std::mutex tracker_status_mutex;
    bool tracker_status =true;

    std::string tracker_status_topic_;
public:
    target_tracker(/* args */);
    void info_topic();
    bool compareBydistance(const  yolo_detect_message::msg::DetectBox & a, const yolo_detect_message::msg::DetectBox& b) {
        
        float half_width = 1920 / 2;
        float half_height = 1080 / 2;
        float a_target_x = a.xmin + a.width / 2.0;
        float a_target_y = a.ymin + a.heigh / 2.0;

        float b_target_x = b.xmin + b.width / 2.0;
        float b_target_y = b.ymin + b.heigh / 2.0;

        float distance_a = sqrt(pow((half_width - a_target_x) , 2) + pow((half_height - a_target_y), 2));
        float distance_b = sqrt(pow((half_width - b_target_x) , 2) + pow((half_height - b_target_y), 2));
        return distance_a < distance_b;
    }
    // uav state
    void state_callback(const mavros_msgs::msg::State::SharedPtr msg);
    // tracker state
    void tracker_object(const yolo_detect_message::msg::DetectBoxs::SharedPtr msg);
    // set armming
    void arm_drone();
    void service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg);
    // set offboard
    void set_offboard_mode();

    void multi_service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg);
    void mutil_state_callback(const mavros_msgs::msg::State::SharedPtr msg);
    // velocity controller
    void timer_loop();
    // reset velocity
    void reset_velocity();
    ~target_tracker();

    void pose_sub(const geometry_msgs::msg::PoseStamped::SharedPtr msg);

    void set_velocity(float x, float y);
    void service_callback(const yolo_detect_message::srv::Service::Request::SharedPtr req,
                        const yolo_detect_message::srv::Service::Response::SharedPtr res);

    void set_velocity_z(float z);
    void declare_parameters()
    {
        this->declare_parameter<std::string>("subscribers.uav_state_sub.topic", "/mavros/state");
        this->declare_parameter<int>("subscribers.uav_state_sub.queue_size", 10);
        
        this->declare_parameter<std::string>("subscribers.trackerid_sub.topic", "/detection_image/result_box");
        this->declare_parameter<int>("subscribers.trackerid_sub.queue_size", 10);
        
        this->declare_parameter<std::string>("subscribers.location_sub.topic", "/mavros/local_position/pose");
        
        this->declare_parameter<std::string>("subscribers.call_server.topic", "drone_target_tracker_server_topic");

        this->declare_parameter<std::string>("client.arming_client.topic", "/mavros/cmd/arming");
        this->declare_parameter<std::string>("client.set_mode_client.topic", "/mavros/set_mode");
        this->declare_parameter<std::string>("client.velocity_pub.topic", "/mavros/setpoint_velocity/cmd_vel_unstamped");

        this->declare_parameter<std::string>("services.calldetectserver.names", "drone_target_tracker_server");

        this->declare_parameter<double>("params.pid_x.kp", 0.5);
        this->declare_parameter<double>("params.pid_x.ki", 0.01);
        this->declare_parameter<double>("params.pid_x.kd", 0.05);

        this->declare_parameter<double>("params.pid_y.kp", 0.5);
        this->declare_parameter<double>("params.pid_y.ki", 0.01);
        this->declare_parameter<double>("params.pid_y.kd", 0.05);

        this->declare_parameter<int>("params.tracker_id.id", 1);
        this->declare_parameter<std::string>("params.tracker_id.names", "tank");
        this->declare_parameter<double>("params.tracker_id.delta_t", 0.1);

        this->declare_parameter<double>("params.max_vel.value", 3.0);

    }

    void read_parameters(){
        uav_state_sub_topic_ = this->get_parameter("subscribers.uav_state_sub.topic").as_string();
        uav_state_sub_queue_size_ = this->get_parameter("subscribers.uav_state_sub.queue_size").as_int();



        trackerid_sub_topic_ = this->get_parameter("subscribers.trackerid_sub.topic").as_string();
        trackerid_sub_queue_size_ = this->get_parameter("subscribers.trackerid_sub.queue_size").as_int();


        locaiton_sub_ = this->get_parameter("subscribers.location_sub.topic").as_string();
        call_server_topic_name = this->get_parameter("subscribers.call_server.topic").as_string();

        arming_client_topic_ = this->get_parameter("client.arming_client.topic").as_string();
        set_mode_client_topic_ = this->get_parameter("client.set_mode_client.topic").as_string();
        velocity_pub_topic_ = this->get_parameter("client.velocity_pub.topic").as_string();

        detect_server_ = this->get_parameter("services.calldetectserver.names").as_string();

        pid_x__Kp = this->get_parameter("params.pid_x.kp").as_double();
        pid_x__Ki = this->get_parameter("params.pid_x.ki").as_double();
        pid_x__Kd = this->get_parameter("params.pid_x.kd").as_double();

        pid_y__Kp = this->get_parameter("params.pid_y.kp").as_double();
        pid_y__Ki = this->get_parameter("params.pid_y.ki").as_double();
        pid_y__Kd = this->get_parameter("params.pid_y.kd").as_double();

        tracker_id = this->get_parameter("params.tracker_id.id").as_int();
        tracker_name_ = this->get_parameter("params.tracker_id.names").as_string();
        delta_t = this->get_parameter("params.tracker_id.delta_t").as_double();

        max_vel = this->get_parameter("params.max_vel.value").as_double();

        RCLCPP_INFO(this->get_logger(), "uav_state_sub_topic: %s", uav_state_sub_topic_.c_str());
     
        RCLCPP_INFO(this->get_logger(), "trackerid_sub_topic: %s", trackerid_sub_topic_.c_str());

        RCLCPP_INFO(this->get_logger(), "arming_client_topic: %s", arming_client_topic_.c_str());
       
        RCLCPP_INFO(this->get_logger(), "set_mode_client_topic: %s", set_mode_client_topic_.c_str());

        RCLCPP_INFO(this->get_logger(), "velocity_pub_topic: %s", velocity_pub_topic_.c_str());
        
        RCLCPP_INFO(this->get_logger(), "tracker_id: %d", tracker_id);
       
        RCLCPP_INFO(this->get_logger(), "detect_server: %s", detect_server_.c_str());
  
        
        RCLCPP_INFO(this->get_logger(), "locaiton_sub: %s", locaiton_sub_.c_str());
     

        if (call_server_topic_name.empty()||uav_state_sub_topic_.empty() || trackerid_sub_topic_.empty() || arming_client_topic_.empty() || set_mode_client_topic_.empty() || velocity_pub_topic_.empty()||detect_server_.empty()||tracker_name_.empty() || locaiton_sub_.empty())
        {
            RCLCPP_ERROR(this->get_logger(), "parameter is empty");
            rclcpp::shutdown();
        }
    }
};

